﻿using DroneLib;
using DroneLib.Communication;
using DroneLib.Vehicle.Models;
using System;
using System.Collections.Generic;
using System.Diagnostics;
using System.IO;
using System.Linq;
using System.Net.Http;
using System.Text;
using System.Threading;
using System.Threading.Tasks;
using static MAVLink;

namespace DroneLib.Vehicles
{
    partial class Vehicle
    {
        readonly char[] _TrimChars = new char[] { ' ', '\0', '\t' };

        bool _UseCommandInt = false;

        /// <summary>
        /// 发送一条 MavCmd 并等待响应
        /// </summary>
        /// <param name="targetComponentId">目标组件号</param>
        /// <param name="command">命令</param>
        /// <param name="responseMsgId">响应的消息号</param>
        /// <param name="timeout">超时时长（毫秒）</param>
        /// <param name="param1">参数1</param>
        /// <param name="param2">参数2</param>
        /// <param name="param3">参数3</param>
        /// <param name="param4">参数4</param>
        /// <param name="param5">参数5</param>
        /// <param name="param6">参数6</param>
        /// <param name="param7">参数7</param>
        /// <returns>响应消息</returns>
        public async Task<MAVLinkMessage> WaitMavCmdResponse(MAV_COMPONENT targetComponentId, MAV_CMD command, MAVLINK_MSG_ID responseMsgId, int timeout, float param1, float param2 = 0.0f, float param3 = 0, float param4 = 0, float param5 = 0, float param6 = 0, float param7 = 0)
        {
            mavlink_command_long_t cmd = new MAVLink.mavlink_command_long_t()
            {
                command = (UInt16)command,
                confirmation = 0,
                param1 = param1,
                param2 = param2,
                param3 = param3,
                param4 = param4,
                param5 = param5,
                param6 = param6,
                param7 = param7,
                target_system = SysId,
                target_component = (byte)targetComponentId,
            };
            return await WaitResponse(MAVLINK_MSG_ID.COMMAND_LONG, cmd, timeout, responseMsgId);
        }

        /// <summary>
        /// 发送一条 MavCmd 并等待响应
        /// </summary>
        /// <param name="targetComponentId">目标组件号</param>
        /// <param name="command">命令</param>
        /// <param name="responseMsgId">响应的消息号</param>
        /// <param name="timeout">超时时长（毫秒）</param>
        /// <param name="param1">参数1</param>
        /// <param name="param2">参数2</param>
        /// <param name="param3">参数3</param>
        /// <param name="param4">参数4</param>
        /// <param name="param5">参数5</param>
        /// <param name="param6">参数6</param>
        /// <param name="param7">参数7</param>
        /// <returns>响应消息</returns>
        public async Task<MAVLinkMessage> WaitMavCmdResponse(MAV_COMPONENT targetComponentId, MAV_CMD command, MAVLINK_MSG_ID responseMsgId, int timeout, MAV_FRAME frame, float param1, float param2 = 0.0f, float param3 = 0, float param4 = 0, float param5 = 0, float param6 = 0, float param7 = 0)
        {

            mavlink_command_int_t cmdInt = new mavlink_command_int_t()
            {
                target_system = this.SysId,
                target_component = (byte)targetComponentId,
                frame = (byte)frame,
                command = (UInt16)command,
                current = 0,
                autocontinue = 0,
                param1 = param1,
                param2 = param2,
                param3 = param3,
                param4 = param4,
                z = param7
            };
            switch (frame)
            {
                case MAV_FRAME.LOCAL_ENU:
                case MAV_FRAME.LOCAL_FLU:
                case MAV_FRAME.LOCAL_FRD:
                case MAV_FRAME.LOCAL_NED:
                case MAV_FRAME.LOCAL_OFFSET_NED:
                    cmdInt.x = (int)(param5 * 1e4);
                    cmdInt.y = (int)(param6 * 1e4);
                    break;
                case MAV_FRAME.GLOBAL:
                case MAV_FRAME.GLOBAL_INT:
                case MAV_FRAME.GLOBAL_RELATIVE_ALT:
                case MAV_FRAME.GLOBAL_RELATIVE_ALT_INT:
                case MAV_FRAME.GLOBAL_TERRAIN_ALT:
                case MAV_FRAME.GLOBAL_TERRAIN_ALT_INT:
                    cmdInt.x = (int)(param5 * 1e7);
                    cmdInt.y = (int)(param6 * 1e7);
                    break;
            }
            return await WaitResponse(MAVLINK_MSG_ID.COMMAND_INT, cmdInt, timeout, responseMsgId);
        }

        /// <summary>
        /// 发送一条 MavCmd 并等待响应
        /// </summary>
        /// <param name="targetComponentId">目标组件号</param>
        /// <param name="command">命令</param>
        /// <param name="timeout">超时时长</param>
        /// <param name="param1">参数1</param>
        /// <param name="param2">参数2</param>
        /// <param name="param3">参数3</param>
        /// <param name="param4">参数4</param>
        /// <param name="param5">参数5</param>
        /// <param name="param6">参数6</param>
        /// <param name="param7">参数7</param>
        /// <returns>响应消息</returns>
        public async Task<MAV_RESULT> WaitMavCmdResult(MAV_COMPONENT targetComponentId, MAV_CMD command, int timeout, float param1, float param2 = 0.0f, float param3 = 0, float param4 = 0, float param5 = 0, float param6 = 0, float param7 = 0)
        {
            var response =await WaitMavCmdResponse(targetComponentId, command, MAVLINK_MSG_ID.COMMAND_ACK, timeout, param1, param2, param3, param4, param5, param6, param7);
            if (response != null)
            {
                mavlink_command_ack_t ack = (mavlink_command_ack_t)response.data;
                var result= (MAV_RESULT)ack.result;
                return result;
            }
            else
                return MAV_RESULT.FAILED;
        }

        DateTime _StartWait;
        int _Timeout = 2000;
        /// <summary>
        /// 发送请求，然后等待响应
        /// </summary>
        /// <param name="requestType">请求消息类型</param>
        /// <param name="request">请求消息体</param>
        /// <param name="timeout">超时时长（毫秒）</param>
        /// <param name="expectedResponseMsgId">期待得到的响应消息列表</param>
        /// <returns>响应消息</returns>
        public async Task<MAVLinkMessage> WaitResponse(MAVLINK_MSG_ID requestType,object request, int timeout, MAVLINK_MSG_ID expectedResponseMsgId)
        {
            if (_MavStream is null)
                return null;

            MAVLinkMessage response = null;
            bool responsed = false;

            RetryTimeout retryTimeout = new RetryTimeout(5, timeout);
            retryTimeout.WorkToDoAsync = async () =>
            {
                SendMessage(requestType, request);
                return true;
            };

            Func<MAVLinkMessage, bool> func = (MAVLinkMessage msg) => 
            {
                if (responsed == false)
                {
                    response = msg;
                    retryTimeout.Complete = true;
                    responsed = true;
                    return true;
                }

                return false;
            };

            if (_MavStream is null)
                return null;


            int id = _MavStream.SubscribeMessage(expectedResponseMsgId, func, SysId, MAVLink.MAV_COMPONENT.MAV_COMP_ID_AUTOPILOT1);
            await retryTimeout.DoWorkAsync();
            _MavStream.UnsubcribeMessage(id);

            return response;
        }

        /// <summary>
        /// 向载具发送一条消息
        /// </summary>
        /// <param name="msgId">消息类型号</param>
        /// <param name="data">消息内容</param>
        public void SendMessage(MAVLINK_MSG_ID msgId,object data)
        {
            if (_MavStream is null)
                return;

            try
            {
                _MavStream.SendMessage(msgId, data);
            }
            catch(Exception ex)
            {
                Console.WriteLine(ex.Message);
            }
        }

        /// <summary>
        /// 下载参数表
        /// </summary>
        /// <param name="timeout">超时时间（毫秒）</param>
        /// <returns>是否取消，参数的总数量，是否全部下载成功</returns>
        public async Task<(bool canceled,int totalCount,bool isSuccess)> DownloadParameters(CancellationTokenSource cts=null,int timeout = 3000)
        {
            
            int lastPercent = -1, percent = 0;
            int totalCount = 0;
            bool canceled = false;
            RetryTimeout retryTimeout = new RetryTimeout(3, timeout);

            Func<MAVLink.MAVLinkMessage, bool> func = (msg) => 
            {
                if (cts != null && cts.IsCancellationRequested)
                {
                    retryTimeout.RetriesCurrent = 999;
                    canceled = true;
                    Console.WriteLine("Param download canceled");
                    return false;
                }

                mavlink_param_value_t rsp = (mavlink_param_value_t)msg.data;
                string paramId = Encoding.ASCII.GetString(rsp.param_id).Trim(_TrimChars);
                _Params[paramId] = rsp;
                totalCount =rsp.param_count;
                if (rsp.param_index < rsp.param_count)
                {
                    percent = (rsp.param_index + 1) * 100 / rsp.param_count;
                    if (percent != lastPercent)
                    {
                        lastPercent = percent;
                        DownloadParamsProgressChanged?.Invoke(this, rsp.param_count, percent);
                    }
                }
                retryTimeout.ResetTimeout();
                retryTimeout.Complete = percent == 100;
                //Console.WriteLine($"{paramId} received, percent= {percent}%");
                return true;
            };

            int id = _MavStream.SubscribeMessage(MAVLINK_MSG_ID.PARAM_VALUE, func, SysId, MAV_COMPONENT.MAV_COMP_ID_AUTOPILOT1);
            retryTimeout.WorkToDoAsync = async() =>
            {
                if(cts!=null && cts.IsCancellationRequested)
                {
                    retryTimeout.RetriesCurrent = 999;
                    canceled = true;
                    Console.WriteLine("Param download canceled");
                    return false;
                }

                mavlink_param_request_list_t req = new mavlink_param_request_list_t(this.SysId, (byte)MAV_COMPONENT.MAV_COMP_ID_ALL);
                SendMessage(MAVLINK_MSG_ID.PARAM_REQUEST_LIST, req);
                return true;
            };
            await retryTimeout.DoWorkAsync();
            _MavStream.UnsubcribeMessage(id);

            return (canceled, totalCount, percent == 100);
        }



        public bool DownloadParametersByMavFTP(CancellationTokenSource cts)
        {
            _Ftp.Progress += (fileName,percent) => 
            {
                DownloadParamsProgressChanged?.Invoke(this, 0, percent);
            };
            var ms = _Ftp.GetFile("@PARAM/param.pck?withdefaults=1", cts, true, 110);
            if (ms != null)
            {
                try
                {
                    var parameters = ParamPacks.Extract(ms);
                    _Params = parameters.ToDictionary(p => Encoding.ASCII.GetString(p.param_id).Trim(_TrimChars));
                    return true;
                }
                catch(Exception ex)
                {
                    Console.WriteLine(ex.Message);
                    return false;
                }
            }
            else
                return false;
        }

        public async Task<(bool success,float value)> ReadParameter(string paramName)
        {
            byte[] paramIdBytes = Encoding.ASCII.GetBytes(paramName.PadRight(16, '\0'));
            mavlink_param_request_read_t req = new mavlink_param_request_read_t(-1, SysId, (byte)MAVLink.MAV_COMPONENT.MAV_COMP_ID_AUTOPILOT1, paramIdBytes);
            
            RetryTimeout retryTimeout = new RetryTimeout(3);
            Func<MAVLinkMessage, bool> func = (rsp) => 
            {
                mavlink_param_value_t paramValue = (mavlink_param_value_t)rsp.data;
                string paramId = Encoding.ASCII.GetString(paramValue.param_id).Trim(_TrimChars);

                bool b = paramName.Equals(paramId);
                if (b)
                {
                    Params[paramId] = paramValue;
                    retryTimeout.Complete = true;
                }
                return b;
            };
            retryTimeout.WorkToDoAsync =async () => 
            {
                SendMessage(MAVLINK_MSG_ID.PARAM_REQUEST_READ, req);
                return true;
            };

            int id = _MavStream.SubscribeMessage(MAVLINK_MSG_ID.PARAM_VALUE, func, this.SysId, MAV_COMPONENT.MAV_COMP_ID_AUTOPILOT1);
            bool success =await retryTimeout.DoWorkAsync();
            _MavStream.UnsubcribeMessage(id);

            return (success, Params[paramName].param_value);
        }

        public async Task<bool> WriteParameter(string paramName, float value/*, MAV_PARAM_TYPE paramType*/)
        {
            if (Params.ContainsKey(paramName) == false)
                return false;

            MAV_PARAM_TYPE paramType = (MAV_PARAM_TYPE)Params[paramName].param_type;

            byte[] paramIdBytes = Encoding.ASCII.GetBytes(paramName.PadRight(16, '\0'));
            mavlink_param_set_t req = new mavlink_param_set_t(value, SysId, (byte)MAVLink.MAV_COMPONENT.MAV_COMP_ID_AUTOPILOT1, paramIdBytes, (byte)paramType);

            RetryTimeout retryTimeout = new RetryTimeout(3,3000);
            bool writeOK = false;
            Func<MAVLinkMessage, bool> func = (rsp) => 
            {
                var paramValue = (mavlink_param_value_t)rsp.data;
                string paramId = Encoding.ASCII.GetString(paramValue.param_id).Trim(_TrimChars);
                if (paramId.Equals(paramName) == false)
                    return false;

                Console.WriteLine("++++++++++++++++++");

                if (paramValue.param_value == value)
                {// 更新参数
                    var param = this.Params[paramName];
                    param.param_value = value;
                    this.Params[paramName] = param;
                    retryTimeout.Complete = true;
                    writeOK = true;
                    return true;
                }
                else
                {
                    retryTimeout.Complete = true;
                    return false;
                }
            };
            retryTimeout.WorkToDoAsync = async () =>
            {
                Console.WriteLine("Send MAVLINK_MSG_ID.PARAM_SET");
                SendMessage(MAVLINK_MSG_ID.PARAM_SET, req);
                return true;
            };
            int id = _MavStream.SubscribeMessage(MAVLINK_MSG_ID.PARAM_VALUE, func, this.SysId, MAV_COMPONENT.MAV_COMP_ID_AUTOPILOT1);
            await retryTimeout.DoWorkAsync();
            _MavStream.UnsubcribeMessage(id);
           
            return writeOK;
        }

        public bool HasParam(string paramName) => Params.ContainsKey(paramName);

        /// <summary>
        /// 下载飞行计划
        /// </summary>
        /// <param name="timeout">超时时间（毫秒）</param>
        /// <returns>应读取的计划行数及实际读取的飞行计划</returns>
        public async Task<(bool canceled,int totalCount,List<mavlink_mission_item_int_t> result)> DownloadMission(CancellationTokenSource cts=null,int timeout = 2000)
        {
            List<mavlink_mission_item_int_t> missionItems = new List<mavlink_mission_item_int_t>();
            mavlink_mission_request_list_t req = new mavlink_mission_request_list_t(this.SysId, (byte)MAV_COMPONENT.MAV_COMP_ID_AUTOPILOT1, (byte)MAV_MISSION_TYPE.MISSION);

            var rsp =await WaitResponse(MAVLINK_MSG_ID.MISSION_REQUEST_LIST, req, timeout, MAVLINK_MSG_ID.MISSION_COUNT);
            if (rsp is null)
                return (false, -1, null);

            var missionCount = (mavlink_mission_count_t)rsp.data;
            int totalCount = missionCount.count;
            UInt16 seq = 0;

            mavlink_mission_request_int_t reqInt = new mavlink_mission_request_int_t(seq, SysId, (byte)MAVLink.MAV_COMPONENT.MAV_COMP_ID_AUTOPILOT1, (byte)MAV_MISSION_TYPE.MISSION);

            RetryTimeout retryTimeout = new RetryTimeout(3, timeout);
            Func<MAVLinkMessage, bool> func = (rspMsg) =>
            {
                mavlink_mission_item_int_t itemInt = (mavlink_mission_item_int_t)rspMsg.data;
                if (itemInt.seq != seq)
                    return false;

                MissionItems.Add(itemInt);

                if (itemInt.seq == totalCount - 1)
                {
                    mavlink_mission_ack_t ack = new mavlink_mission_ack_t(SysId, (byte)MAV_COMPONENT.MAV_COMP_ID_AUTOPILOT1, (byte)MAV_MISSION_RESULT.MAV_MISSION_ACCEPTED, (byte)MAV_MISSION_TYPE.MISSION);
                    _MavStream.SendMessage(MAVLINK_MSG_ID.MISSION_ACK, ack);
                    retryTimeout.Complete = true;
                }

                reqInt.seq = ++seq;
                SendMessage(MAVLINK_MSG_ID.MISSION_REQUEST_INT, reqInt);
                retryTimeout.ResetTimeout();
                return true;
            };
            int id = _MavStream.SubscribeMessage(MAVLINK_MSG_ID.MISSION_ITEM_INT, func, SysId, MAV_COMPONENT.MAV_COMP_ID_AUTOPILOT1);
            
            retryTimeout.WorkToDoAsync =async () => 
            {
                if(cts!=null && cts.IsCancellationRequested)
                {
                    retryTimeout.RetriesCurrent = 999;
                    retryTimeout.Canceled = true;
                    Console.WriteLine("Download mission canceled");
                    return false;
                }

                SendMessage(MAVLINK_MSG_ID.MISSION_REQUEST_INT, reqInt);
                return true;
            };
            await retryTimeout.DoWorkAsync();
            _MavStream.UnsubcribeMessage(id);

            return (retryTimeout.Canceled, totalCount, missionItems);
        }

        /// <summary>
        /// 上传航线
        /// </summary>
        /// <param name="filePath">航线文件路径</param>
        /// <returns>是否上传成功，航线文件</returns>
        public async Task<(bool isSuccess,WaypointFile file,string err)> UploadMission(string filePath, CancellationTokenSource cts = null)
        {
            var file = WaypointFile.Load(filePath);
            if (file == null)
                return (false, null,"文件非法");

            var result = await UploadMission(file, cts);
            return (result.isSuccess, file, result.err);
        }

        /// <summary>
        /// 上传航线文件
        /// </summary>
        /// <param name="file">待上传的航线文件</param>
        /// <returns>是否上传成功</returns>
        public async Task<(bool isSuccess, string err)> UploadMission(WaypointFile file, CancellationTokenSource cts = null)
        {
            (bool, string err) result;

            mavlink_mission_count_t req = new mavlink_mission_count_t((UInt16)(file.MissionItemInts.Count), SysId, (byte)MAV_COMPONENT.MAV_COMP_ID_AUTOPILOT1, (byte)MAV_MISSION_TYPE.MISSION);

            RetryTimeout retryTimeout = new RetryTimeout(3);
            bool useInt = false;
            int lastSeq = -1;
            int lastPercent = -1;
            int percent = 0;
            bool isSuccess = false;
            string err = string.Empty;
            Func<MAVLinkMessage, bool> func = (rspMsg) =>
            {
                if(rspMsg.data is mavlink_mission_ack_t)
                {
                    //mavlink_mission_ack_t ack = (mavlink_mission_ack_t)rspMsg.data;
                    isSuccess = true;
                    UploadMissionProgressChanged?.Invoke(this, 100);
                }    

                object reqNext = null;
                int seq = 0;
                if( rspMsg.data is mavlink_mission_request_t)
                {
                    mavlink_mission_request_t r = (mavlink_mission_request_t)rspMsg.data;
                    seq = r.seq;
                    if (lastSeq != seq)
                    {
                        lastSeq = seq;
                    }
                    else
                        useInt = true;
                }
                else if(rspMsg.data is mavlink_mission_request_int_t)
                {
                    mavlink_mission_request_int_t r = (mavlink_mission_request_int_t)rspMsg.data;
                    seq = r.seq;
                    useInt = true;
                }

                MAVLink.MAVLINK_MSG_ID msgId;
                if(useInt)
                {
                    msgId = MAVLINK_MSG_ID.MISSION_ITEM_INT;
                    reqNext = file.MissionItemInts[seq];
                }
                else
                {
                    msgId = MAVLINK_MSG_ID.MISSION_ITEM;
                    reqNext =file.MissionItems[seq];
                }

                SendMessage(msgId, reqNext);
                percent = seq * 100 / file.MissionItems.Count;
                if(percent!=lastPercent)
                {
                    lastPercent = percent;
                    UploadMissionProgressChanged?.Invoke(this, percent);
                }
                retryTimeout.ResetTimeout();
                return true;
            };
            retryTimeout.WorkToDoAsync = async() =>
            {
                if(cts!=null && cts.IsCancellationRequested)
                {
                    retryTimeout.RetriesCurrent = int.MaxValue - 1;
                    retryTimeout.Canceled = true;
                    Console.WriteLine("Upliad mission canceled.");
                    return false;
                }

                SendMessage(MAVLINK_MSG_ID.MISSION_COUNT, req);
                return true;
            };
            int id1 = _MavStream.SubscribeMessage(MAVLINK_MSG_ID.MISSION_REQUEST, func, this.SysId, MAV_COMPONENT.MAV_COMP_ID_AUTOPILOT1);
            int id2 = _MavStream.SubscribeMessage(MAVLINK_MSG_ID.MISSION_REQUEST_INT, func, this.SysId, MAV_COMPONENT.MAV_COMP_ID_AUTOPILOT1);
            int id3 = _MavStream.SubscribeMessage(MAVLINK_MSG_ID.MISSION_ACK, func, this.SysId, MAV_COMPONENT.MAV_COMP_ID_AUTOPILOT1);
            await retryTimeout.DoWorkAsync();
            _MavStream.UnsubcribeMessage(id1);
            _MavStream.UnsubcribeMessage(id2);
            _MavStream.UnsubcribeMessage(id3);


            return (isSuccess, err);
        }


        /// <summary>
        /// 设置模式
        /// </summary>
        /// <param name="modeCode">模式代码</param>
        /// <returns></returns>
        protected virtual async Task<bool> SetModeByModeCode(UInt32 modeCode)
        {
            if (this._ModeCode == modeCode)
                return true;

            // 新方法
            byte baseMode = (byte)MAV_MODE_FLAG.CUSTOM_MODE_ENABLED;
            var result =await WaitMavCmdResult(MAV_COMPONENT.MAV_COMP_ID_AUTOPILOT1, MAV_CMD.DO_SET_MODE, 3000, baseMode, modeCode, 0, 0, 0, 0, 0);
            if (result == MAV_RESULT.ACCEPTED)
                return true;

            // 老方法
            mavlink_set_mode_t req = new mavlink_set_mode_t(modeCode, SysId, (byte)MAV_MODE_FLAG.CUSTOM_MODE_ENABLED);

            _MavStream.SendMessage(MAVLINK_MSG_ID.SET_MODE, req);
            Thread.Sleep(100);
            _MavStream.SendMessage(MAVLINK_MSG_ID.SET_MODE, req);
            Thread.Sleep(100);
            return true;
        }

        public abstract Task<bool> ReturnToLaunch();

        public abstract Task<bool> Auto();

        public async Task<bool> MissionStart()
        {
            return await WaitMavCmdResult(MAV_COMPONENT.MAV_COMP_ID_AUTOPILOT1, MAV_CMD.MISSION_START, 1000, 0) == MAV_RESULT.ACCEPTED;
        }

        public bool ToggleSafety(bool on)
        {
            throw new NotImplementedException();
            //var customMode=Sensors.MotorControlEnabled&&Sensors.
        }

        protected async Task<bool> ArmDisarm(bool arm, bool forece = false)
        {
            //if(arm)
            //{
            //    switch(this.Firmware)
            //    {
            //        case DroneLib.Vehicle.Firmwares.ArduPlane:
            //            SetMode("FLY_BY_WIRE_A");
            //            break;
            //        case DroneLib.Vehicle.Firmwares.ArduCopter2:
            //            SetMode("POSHOLD");
            //            break;
            //    }
            //}

            return await WaitMavCmdResult(MAV_COMPONENT.MAV_COMP_ID_AUTOPILOT1, MAV_CMD.COMPONENT_ARM_DISARM, 1000, arm ? 1 : 0,forece? 21196 : 0) == MAV_RESULT.ACCEPTED;
        }

        public async Task<bool> Arm(bool force = false)
        {
            return await ArmDisarm(true, force);
        }

        public async Task<bool> Disarm(bool force = false)
        {
            return await ArmDisarm(false, force);
        }

        /// <summary>
        /// 将指定位置设置为返航点
        /// </summary>
        /// <param name="home">返航点坐标（纬度，经度，高度）</param>
        /// <returns>是否设置成功</returns>
        public async Task<bool> SetHome(PointLatLngAlt home)
        {
            var result=await WaitMavCmdResult(MAV_COMPONENT.MAV_COMP_ID_AUTOPILOT1, MAV_CMD.DO_SET_HOME, 2000, 0, 0, 0, 0, (float)home.Lat, (float)home.Lng, (float)home.Alt);
            return result == MAV_RESULT.ACCEPTED;

            //mavlink_set_home_position_t msg = new mavlink_set_home_position_t()
            //{
            //    latitude=(int)(home.Latitude*1e7),
            //    longitude= (int)(home.Longitude * 1e7),
            //    altitude= (int)(home.Altitude * 1e3),
            //    target_system=SysId,
            //};
            //SendMessage(MAVLINK_MSG_ID.SET_HOME_POSITION, msg);
            //return true;
            //return WaitMavCmdResult(MAV_COMPONENT.MAV_COMP_ID_AUTOPILOT1, MAV_CMD.DO_SET_HOME, 1000, 0, 0, 0, 0, (float)home.Latitude, (float)home.Longitude, (float)home.Altitude) == MAV_RESULT.ACCEPTED;
        }

        /// <summary>
        /// 将飞行器当前位置设置为返航点
        /// </summary>
        /// <returns>是否设置成功</returns>
        public async Task<bool> SetHome()
        {
            return await WaitMavCmdResult(MAV_COMPONENT.MAV_COMP_ID_AUTOPILOT1, MAV_CMD.DO_SET_HOME, 1000, 1) == MAV_RESULT.ACCEPTED;
        }

        /// <summary>
        /// 获取返航点未知
        /// </summary>
        /// <returns>是否成功，返航点的纬度、经度、高度（MSL）、X坐标、Y坐标、Z坐标</returns>
        public async Task< (bool isSuccess, float lat, float lng, float alt, float x, float y, float z)> GetHome()
        {
            bool responsed = false;
            float lat = 0, lng = 0, alt = 0, x = 0, y = 0, z = 0;
            var rspMsg =await WaitMavCmdResponse(MAV_COMPONENT.MAV_COMP_ID_AUTOPILOT1, MAV_CMD.REQUEST_MESSAGE, MAVLINK_MSG_ID.HOME_POSITION, 1000, (byte)MAVLINK_MSG_ID.HOME_POSITION);
            if (rspMsg != null)
            {
                mavlink_home_position_t rsp = (mavlink_home_position_t)rspMsg.data;
                responsed = true;
                lat = rsp.latitude / 1e7f;
                lng = rsp.longitude / 1e7f;
                alt = rsp.altitude / 1000;
                x = rsp.x;
                y = rsp.y;
                z = rsp.z;
            }
            else
            {
                var rspMsg2 =await WaitMavCmdResponse(MAV_COMPONENT.MAV_COMP_ID_AUTOPILOT1, MAV_CMD.GET_HOME_POSITION, MAVLINK_MSG_ID.HOME_POSITION, 1000, 0);
                if(rspMsg2!=null)
                {
                    mavlink_home_position_t rsp = (mavlink_home_position_t)rspMsg2.data;
                    responsed = true;
                    lat = rsp.latitude / 1e7f;
                    lng = rsp.longitude / 1e7f;
                    alt = rsp.altitude / 1000;
                    x = rsp.x;
                    y = rsp.y;
                    z = rsp.z;
                }
            }

            if (responsed)
            {
                this.HomeLocation.Lat = lat;
                this.HomeLocation.Lng = lng;
                this.HomeLocation.Alt = alt;
            }

            return (responsed, lat, lng, alt, x, y, z);
        }

        public async Task<bool> GotoWapoint(int waypointNumber,bool resetMission = false)
        {
            var result =await WaitMavCmdResult(MAV_COMPONENT.MAV_COMP_ID_AUTOPILOT1, MAV_CMD.DO_SET_MISSION_CURRENT, 1000, waypointNumber, resetMission ? 1 : 0);
            return result == MAV_RESULT.ACCEPTED;
        }

        public abstract Task<bool> GotoLatLng(float lat, float lng, float alt, bool ignoreYaw, MAV_FRAME frame = MAVLink.MAV_FRAME.GLOBAL_RELATIVE_ALT_INT);

        public abstract Task<bool> GotoXY(float x, float y, float z, bool ignoreYaw = true);

        public abstract Task<bool> SetYaw(float yaw);


        /// <summary>
        /// 设置位置更新频率
        /// </summary>
        /// <param name="rate">更新频率（Hz）</param>
        /// <returns>是否设置成功</returns>
        public async Task<bool> SetPositionUpdateRate(int rate)
        {
            return await WaitMavCmdResult(MAV_COMPONENT.MAV_COMP_ID_AUTOPILOT1, MAV_CMD.SET_MESSAGE_INTERVAL, 1000, (int)MAVLINK_MSG_ID.GLOBAL_POSITION_INT, 1000000 / rate) == MAV_RESULT.ACCEPTED;
        }

        public async Task RequestStream(bool disabled=false)
        {
            if(this.MavlinkVersion==1)
            {
                RequestStreamOld(disabled);
                return;
            }
            // 以下可用，但是 MP 里用的还是已弃用的方法 REQUEST_DATA_STREAM

            // GPS SatCount, 1 Hz
            await WaitMavCmdResult(MAV_COMPONENT.MAV_COMP_ID_AUTOPILOT1, MAV_CMD.SET_MESSAGE_INTERVAL, 1000, (float)MAVLINK_MSG_ID.GPS_STATUS, disabled ? -1 : 1000000, 0, 0, 0, 0, 0);

            // GPS_RAW_INT, 10 Hz
            await WaitMavCmdResult(MAV_COMPONENT.MAV_COMP_ID_AUTOPILOT1, MAV_CMD.SET_MESSAGE_INTERVAL, 1000, (float)MAVLINK_MSG_ID.GPS_RAW_INT, disabled ? -1 : 1000000 /10, 0, 0, 0, 0, 0);

            // SYSTEM_TIME, 1 Hz
            await WaitMavCmdResult(MAV_COMPONENT.MAV_COMP_ID_AUTOPILOT1, MAV_CMD.SET_MESSAGE_INTERVAL, 1000, (float)MAVLINK_MSG_ID.SYSTEM_TIME, disabled ? -1 : 1000000 /1, 0, 0, 0, 0, 0);

            // ATTITUDE, 5 Hz
            await WaitMavCmdResult(MAV_COMPONENT.MAV_COMP_ID_AUTOPILOT1, MAV_CMD.SET_MESSAGE_INTERVAL, 1000, (float)MAVLINK_MSG_ID.ATTITUDE, disabled ? -1 : 1000000 / 5, 0, 0, 0, 0, 0);

            // SYS_STATUS, 2Hz
            await WaitMavCmdResult(MAV_COMPONENT.MAV_COMP_ID_AUTOPILOT1, MAV_CMD.SET_MESSAGE_INTERVAL, 1000, (float)MAVLINK_MSG_ID.SYS_STATUS, disabled ? -1 : 1000000 / 2, 0, 0, 0, 0, 0);

            // GLOBAL_POSITION_INT, 10 Hz
            await WaitMavCmdResult(MAV_COMPONENT.MAV_COMP_ID_AUTOPILOT1, MAV_CMD.SET_MESSAGE_INTERVAL, 1000, (float)MAVLINK_MSG_ID.GLOBAL_POSITION_INT, disabled ? -1 : 1000000 / 10, 0, 0, 0, 0, 0);

            // NAV_CONTROLLER_OUTPUT, 5 Hz
            await WaitMavCmdResult(MAV_COMPONENT.MAV_COMP_ID_AUTOPILOT1, MAV_CMD.SET_MESSAGE_INTERVAL, 1000, (float)MAVLINK_MSG_ID.NAV_CONTROLLER_OUTPUT, disabled ? -1 : 1000000 / 5, 0, 0, 0, 0, 0);

            // VFR_HUD, 5Hz
            await WaitMavCmdResult(MAV_COMPONENT.MAV_COMP_ID_AUTOPILOT1, MAV_CMD.SET_MESSAGE_INTERVAL, 1000, (float)MAVLINK_MSG_ID.VFR_HUD, disabled ? -1 : 1000000 / 5, 0, 0, 0, 0, 0);

            // RADIO_STATUS, 1 Hz
            await WaitMavCmdResult(MAV_COMPONENT.MAV_COMP_ID_AUTOPILOT1, MAV_CMD.SET_MESSAGE_INTERVAL, 1000, (float)MAVLINK_MSG_ID.RADIO_STATUS, disabled ? -1 : 1000000 / 1, 0, 0, 0, 0, 0);

            // POWER_STATUS, 1 Hz
            await WaitMavCmdResult(MAV_COMPONENT.MAV_COMP_ID_AUTOPILOT1, MAV_CMD.SET_MESSAGE_INTERVAL, 1000, (float)MAVLINK_MSG_ID.POWER_STATUS, disabled ? -1 : 1000000 / 1, 0, 0, 0, 0, 0);

            // BATTERY_STATUS, 1 Hz
            await WaitMavCmdResult(MAV_COMPONENT.MAV_COMP_ID_AUTOPILOT1, MAV_CMD.SET_MESSAGE_INTERVAL, 1000, (float)MAVLINK_MSG_ID.BATTERY_STATUS, disabled ? -1 : 1000000 / 1, 0, 0, 0, 0, 0);

            // EKF_STATUS_REPORT, 1 Hz
            await WaitMavCmdResult(MAV_COMPONENT.MAV_COMP_ID_AUTOPILOT1, MAV_CMD.SET_MESSAGE_INTERVAL, 1000, (float)MAVLINK_MSG_ID.EKF_STATUS_REPORT, disabled ? -1 : 1000000 / 1, 0, 0, 0, 0, 0);

            // VIBRATION, 1 Hz
            await WaitMavCmdResult(MAV_COMPONENT.MAV_COMP_ID_AUTOPILOT1, MAV_CMD.SET_MESSAGE_INTERVAL, 1000, (float)MAVLINK_MSG_ID.VIBRATION, disabled ? -1 : 1000000 / 1, 0, 0, 0, 0, 0);

            // STATUSTEXT, 1 Hz
            await WaitMavCmdResult(MAV_COMPONENT.MAV_COMP_ID_AUTOPILOT1, MAV_CMD.SET_MESSAGE_INTERVAL, 1000, (float)MAVLINK_MSG_ID.STATUSTEXT, disabled ? -1 : 1000000 / 1, 0, 0, 0, 0, 0);

            // RADIO_STATUS, 1 Hz
            await WaitMavCmdResult(MAV_COMPONENT.MAV_COMP_ID_AUTOPILOT1, MAV_CMD.SET_MESSAGE_INTERVAL, 1000, (float)MAVLINK_MSG_ID.RADIO_STATUS, disabled ? -1 : 1000000 / 1, 0, 0, 0, 0, 0);

            // SERVO_OUTPUT_RAW, 5Hz
            await WaitMavCmdResult(MAV_COMPONENT.MAV_COMP_ID_AUTOPILOT1, MAV_CMD.SET_MESSAGE_INTERVAL, 1000, (float)MAVLINK_MSG_ID.SERVO_OUTPUT_RAW, disabled ? -1 : 1000000 / 5, 0, 0, 0, 0, 0);

            // MISSION_CURRENT
            await WaitMavCmdResult(MAV_COMPONENT.MAV_COMP_ID_AUTOPILOT1, MAV_CMD.SET_MESSAGE_INTERVAL, 1000, (float)MAVLINK_MSG_ID.MISSION_CURRENT, disabled ? -1 : 1000000 / 2, 0, 0, 0, 0, 0);

            // HOME_POSITION
            await WaitMavCmdResult(MAV_COMPONENT.MAV_COMP_ID_AUTOPILOT1, MAV_CMD.SET_MESSAGE_INTERVAL, 1000, (float)MAVLINK_MSG_ID.HOME_POSITION, disabled ? -1 : 1000000 / 1, 0, 0, 0, 0, 0);

            // RC_CHANNELS
            await WaitMavCmdResult(MAV_COMPONENT.MAV_COMP_ID_AUTOPILOT1, MAV_CMD.SET_MESSAGE_INTERVAL, 1000, (float)MAVLINK_MSG_ID.RC_CHANNELS, disabled ? -1 : 1000000 / 3, 0, 0, 0, 0, 0);
        }

        private void RequestStreamOld(bool disabled=false)
        {
            if(disabled)
            {
                mavlink_request_data_stream_t req = new mavlink_request_data_stream_t
                {
                    target_system = this.SysId,
                    target_component =(byte) MAVLink.MAV_COMPONENT.MAV_COMP_ID_AUTOPILOT1,
                    req_message_rate = 10,
                    start_stop = 0,
                    req_stream_id = 0
                };

                // no error on bad
                try
                {
                    SendMessage(MAVLINK_MSG_ID.REQUEST_DATA_STREAM, req);
                    Thread.Sleep(20);
                    SendMessage(MAVLINK_MSG_ID.REQUEST_DATA_STREAM, req);
                    Thread.Sleep(20);
                    SendMessage(MAVLINK_MSG_ID.REQUEST_DATA_STREAM, req);
                    Console.WriteLine("Stop All Done");
                }
                catch
                {
                }
            }
            else
            {
                try
                {
                    RequestDatastream(MAV_DATA_STREAM.EXTENDED_STATUS);
                    RequestDatastream(MAV_DATA_STREAM.POSITION);
                    RequestDatastream(MAV_DATA_STREAM.EXTRA1);
                    RequestDatastream(MAV_DATA_STREAM.EXTRA2);
                    RequestDatastream(MAV_DATA_STREAM.EXTRA3);
                    RequestDatastream(MAV_DATA_STREAM.RAW_SENSORS);
                    RequestDatastream(MAV_DATA_STREAM.RC_CHANNELS);
                }
                catch
                {
                    Console.WriteLine("Failed to request rates");
                }
            }
        }

        private void RequestDatastream(MAV_DATA_STREAM id, int sysid = -1, int compid = -1)
        {
            sysid = this.SysId;
            compid = (byte)MAVLink.MAV_COMPONENT.MAV_COMP_ID_AUTOPILOT1;

            int hzrate = 0;

            switch (id)
            {
                case MAV_DATA_STREAM.ALL:
                    break;

                case MAV_DATA_STREAM.EXTENDED_STATUS:
                    hzrate = 2;
                    break;

                case MAV_DATA_STREAM.EXTRA1:
                    hzrate = 8;
                    break;

                case MAV_DATA_STREAM.EXTRA2:
                    hzrate = 8;
                    break;

                case MAV_DATA_STREAM.EXTRA3:
                    hzrate = 4;
                    break;

                case MAV_DATA_STREAM.POSITION:
                    hzrate = 2;
                    break;

                case MAV_DATA_STREAM.RAW_CONTROLLER:
                    hzrate = 2;
                    break;

                case MAV_DATA_STREAM.RAW_SENSORS:
                    hzrate = 2;
                    break;

                case MAV_DATA_STREAM.RC_CHANNELS:
                    hzrate = 2;
                    break;
            }

            if (hzrate == 0)
            {
                return;
            }
            GetDatastream((byte)sysid, (byte)compid, id, hzrate);
        }

        private void GetDatastream(byte sysid, byte compid, MAV_DATA_STREAM id, int hzrate)
        {
            if (hzrate == -1)
                return;

            mavlink_request_data_stream_t req = new mavlink_request_data_stream_t
            {
                target_system = sysid,
                target_component = compid,
                req_message_rate = (byte)hzrate,
                start_stop = 1,
                req_stream_id = (byte)id
            };

            // send each one twice.
            SendMessage(MAVLINK_MSG_ID.REQUEST_DATA_STREAM, req);
            Thread.Sleep(20);
            SendMessage(MAVLINK_MSG_ID.REQUEST_DATA_STREAM, req);
        }

        public async Task<bool> SetServo(int channel,int pwm)
        {
            return await WaitMavCmdResult(MAV_COMPONENT.MAV_COMP_ID_AUTOPILOT1, MAV_CMD.DO_SET_SERVO, 1000, channel, pwm) == MAV_RESULT.ACCEPTED;
        }

        public async Task<bool> DoLevelCalibration()
        {
            return await WaitMavCmdResult(MAV_COMPONENT.MAV_COMP_ID_AUTOPILOT1, MAV_CMD.PREFLIGHT_CALIBRATION, 1000, 0, 0, 0, 0, 2, 0, 0) == MAV_RESULT.ACCEPTED;
        }

        public async Task<bool> Reboot(int timeout = 1000)
        {
            if (timeout > 0)
            {
                var result =await WaitMavCmdResult(MAV_COMPONENT.MAV_COMP_ID_AUTOPILOT1, MAV_CMD.PREFLIGHT_REBOOT_SHUTDOWN, timeout, 1);
                bool b = result == MAV_RESULT.ACCEPTED;
                if (b)
                    RebootRequired = false;
                return b;
            }
            else
            {
                mavlink_command_long_t cmd = new MAVLink.mavlink_command_long_t()
                {
                    command = (UInt16)MAV_CMD.PREFLIGHT_REBOOT_SHUTDOWN,
                    confirmation = 0,
                    param1 = 1,
                    param2 = 0,
                    param3 = 0,
                    param4 = 0,
                    param5 = 0,
                    param6 = 0,
                    param7 = 0,
                    target_system = SysId,
                    target_component = (byte) MAV_COMPONENT.MAV_COMP_ID_AUTOPILOT1,
                };
                //return WaitResponse(MAVLINK_MSG_ID.COMMAND_LONG, cmd, timeout, responseMsgId);
                SendMessage(MAVLINK_MSG_ID.COMMAND_LONG, cmd);
                return true;
            }
        }

        private int inject_seq_no = 0;
        /// <summary>
        /// used to inject data into the gps ie rtcm/sbp/ubx
        /// </summary>
        /// <param name="data"></param>
        public virtual void InjectGnssData(byte[] data, ushort length, bool rtcm_message = true)
        {
            if (_MavStream is null)
                return;

            if (data == null) throw new ArgumentNullException(nameof(data));
            // new message
            if (rtcm_message)
            {
                mavlink_gps_rtcm_data_t gps = new mavlink_gps_rtcm_data_t();
                var msglen = 180;

                if (length > msglen * 4)
                {
                    Debug.WriteLine("Message too large " + length);
                    return;
                }

                // number of packets we need, including a termination packet if needed
                var nopackets = (length % msglen) == 0 ? length / msglen + 1 : (length / msglen) + 1;

                if (nopackets >= 4)
                    nopackets = 4;

                // flags = isfrag(1)/frag(2)/seq(5)

                for (int a = 0; a < nopackets; a++)
                {
                    // check if its a fragment
                    if (nopackets > 1)
                        gps.flags = 1;
                    else
                        gps.flags = 0;

                    // add fragment number
                    gps.flags += (byte)((a & 0x3) << 1);

                    // add seq number
                    gps.flags += (byte)((inject_seq_no & 0x1f) << 3);

                    // create the empty buffer
                    gps.data = new byte[msglen];

                    // calc how much data we are copying
                    int copy = Math.Min(length - a * msglen, msglen);

                    // copy the data
                    Array.Copy(data, a * msglen, gps.data, 0, copy);

                    // set the length
                    gps.len = (byte)copy;

                    //generatePacket((byte)MAVLINK_MSG_ID.GPS_RTCM_DATA, gps, sysid, compid);
                    SendMessage(MAVLINK_MSG_ID.GPS_RTCM_DATA, gps);
                }

                inject_seq_no++;
            }
            else
            {
                mavlink_gps_inject_data_t gps = new mavlink_gps_inject_data_t();
                var msglen = 110;

                var len = (length % msglen) == 0 ? length / msglen : (length / msglen) + 1;

                for (int a = 0; a < len; a++)
                {
                    gps.data = new byte[msglen];

                    int copy = Math.Min(length - a * msglen, msglen);

                    Array.Copy(data, a * msglen, gps.data, 0, copy);
                    gps.len = (byte)copy;
                    gps.target_component = (byte)MAV_COMPONENT.MAV_COMP_ID_AUTOPILOT1;
                    gps.target_system = this.SysId;

                    //generatePacket((byte)MAVLINK_MSG_ID.GPS_INJECT_DATA, gps, sysid, compid);
                    _MavStream.SendMessage(MAVLINK_MSG_ID.GPS_INJECT_DATA, gps);
                }
            }
        }

        public void ManualControl(params Int16[] channelValues)
        {
            Int16
                roll = channelValues[0],
                pitch = channelValues[1],
                throttle = channelValues[2],
                yaw = channelValues[3];

            MAVLink.mavlink_manual_control_t manual = new mavlink_manual_control_t(pitch, roll, throttle, yaw, 0, this.SysId, 0, 0, 0, 0);

            SendMessage(MAVLINK_MSG_ID.MANUAL_CONTROL, manual);
        }
    }
}
